<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "https://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.17"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>OpenLoong Dynamics Control: gait_scheduler.h Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/x-mathjax-config">
  MathJax.Hub.Config({
    extensions: ["tex2jax.js"],
    jax: ["input/TeX","output/HTML-CSS"],
});
</script>
<script type="text/javascript" async="async" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.5/MathJax.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">OpenLoong Dynamics Control
   &#160;<span id="projectnumber">v0.1.0</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.17 -->
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
var searchBox = new SearchBox("searchBox", "search",false,'Search');
/* @license-end */
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
$(function() {
  initMenu('',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
/* @license-end */</script>
<div id="main-nav"></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_692f640cc74363071c5c0e14282656e9.html">algorithm</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">gait_scheduler.h</div>  </div>
</div><!--header-->
<div class="contents">
<div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;<span class="comment">/*</span></div>
<div class="line"><a name="l00002"></a><span class="lineno">    2</span>&#160;<span class="comment">This is part of OpenLoong Dynamics Control, an open project for the control of biped robot,</span></div>
<div class="line"><a name="l00003"></a><span class="lineno">    3</span>&#160;<span class="comment">Copyright (C) 2024 Humanoid Robot (Shanghai) Co., Ltd, under Apache 2.0.</span></div>
<div class="line"><a name="l00004"></a><span class="lineno">    4</span>&#160;<span class="comment">Feel free to use in any purpose, and cite OpenLoong-Dynamics-Control in any style, to contribute to the advancement of the community.</span></div>
<div class="line"><a name="l00005"></a><span class="lineno">    5</span>&#160;<span class="comment"> &lt;https://gitee.com/panda_23/openloong-dyn-control.git&gt;</span></div>
<div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="comment"> &lt;web@openloong.org.cn&gt;</span></div>
<div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="comment">*/</span></div>
<div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="preprocessor">#pragma once</span></div>
<div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160; </div>
<div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="preprocessor">#include &quot;data_bus.h&quot;</span></div>
<div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="preprocessor">#include &lt;Eigen/Dense&gt;</span></div>
<div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;<span class="preprocessor">#include &quot;useful_math.h&quot;</span></div>
<div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160; </div>
<div class="line"><a name="l00019"></a><span class="lineno"><a class="line" href="classGaitScheduler.html">   19</a></span>&#160;<span class="keyword">class </span><a class="code" href="classGaitScheduler.html">GaitScheduler</a> {</div>
<div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;<span class="keyword">public</span>:</div>
<div class="line"><a name="l00021"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#a3473035ec27c4cdee3d04b1bd156ac0e">   21</a></span>&#160;    <span class="keywordtype">bool</span> <a class="code" href="classGaitScheduler.html#a3473035ec27c4cdee3d04b1bd156ac0e">isIni</a>{<span class="keyword">false</span>};          </div>
<div class="line"><a name="l00022"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#a1f1a221fb6fa50a17a941990cb8a428f">   22</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="classGaitScheduler.html#a1f1a221fb6fa50a17a941990cb8a428f">phi</a>{0};              </div>
<div class="line"><a name="l00023"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#ae7e444a24e0ed19c0a52b8832220538f">   23</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="classGaitScheduler.html#ae7e444a24e0ed19c0a52b8832220538f">tSwing</a>{0.4};         </div>
<div class="line"><a name="l00024"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#afe24a16f3a4b9e4f5db04056b3cdc349">   24</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="classGaitScheduler.html#afe24a16f3a4b9e4f5db04056b3cdc349">dt</a>{0.001};           </div>
<div class="line"><a name="l00025"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#a91185099fb4d6ad3154c01426cd841dc">   25</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="classGaitScheduler.html#a91185099fb4d6ad3154c01426cd841dc">FzThrehold</a>{200};     </div>
<div class="line"><a name="l00026"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#a23b4fe5d102f13755d2b14950bc13e3b">   26</a></span>&#160;    <span class="keywordtype">double</span> <a class="code" href="classGaitScheduler.html#a23b4fe5d102f13755d2b14950bc13e3b">Fz_L_m</a>{0},           </div>
<div class="line"><a name="l00027"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#ab43cec46486e1e5f3cd6e59a6c162ac7">   27</a></span>&#160;        <a class="code" href="classGaitScheduler.html#ab43cec46486e1e5f3cd6e59a6c162ac7">Fz_R_m</a>{0};              </div>
<div class="line"><a name="l00028"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#af10903d6376bdc95bfe49c41ff951017">   28</a></span>&#160;    <a class="code" href="structDataBus.html#a0dcb8a4552bb6b37259a61d601cd7d7e">DataBus::LegState</a> <a class="code" href="classGaitScheduler.html#af10903d6376bdc95bfe49c41ff951017">legState</a>, </div>
<div class="line"><a name="l00029"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#a7593fedd09b2c3897b9a81db2bb0560a">   29</a></span>&#160;        <a class="code" href="classGaitScheduler.html#a7593fedd09b2c3897b9a81db2bb0560a">legStateNext</a>;           </div>
<div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160; </div>
<div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;    <a class="code" href="classGaitScheduler.html#ae80429157cc4a030426a9776a3c087a5">GaitScheduler</a>(<span class="keywordtype">double</span> tSwingIn, <span class="keywordtype">double</span> dtIn);</div>
<div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;    <span class="keywordtype">void</span> dataBusRead(<span class="keyword">const</span> <a class="code" href="structDataBus.html">DataBus</a> &amp;robotState);</div>
<div class="line"><a name="l00038"></a><span class="lineno">   38</span>&#160;    <span class="keywordtype">void</span> dataBusWrite(<a class="code" href="structDataBus.html">DataBus</a> &amp;robotState);</div>
<div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;    <span class="keywordtype">void</span> <a class="code" href="classGaitScheduler.html#aa23623812735c74e6e97554489599790">step</a>();</div>
<div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;    <span class="keywordtype">void</span> <a class="code" href="classGaitScheduler.html#a81aa9cc7644286cb50f5f7d2c066d5b3">stop</a>();</div>
<div class="line"><a name="l00049"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#a614ce3fed948dcc2746f21202437df62">   49</a></span>&#160;    Eigen::VectorXd <a class="code" href="classGaitScheduler.html#a614ce3fed948dcc2746f21202437df62">FLest</a>,    </div>
<div class="line"><a name="l00050"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#abbd1d35129e19e01297f883b69f6cbd3">   50</a></span>&#160;        <a class="code" href="classGaitScheduler.html#abbd1d35129e19e01297f883b69f6cbd3">FRest</a>;                </div>
<div class="line"><a name="l00051"></a><span class="lineno"><a class="line" href="classGaitScheduler.html#a5c644ded833c2b83911d70265a51a12c">   51</a></span>&#160;    Eigen::VectorXd <a class="code" href="classGaitScheduler.html#a5c644ded833c2b83911d70265a51a12c">torJoint</a>; </div>
<div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;<span class="keyword">private</span>:</div>
<div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;    Eigen::VectorXd fe_r_pos_W,       </div>
<div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;        fe_l_pos_W,                   </div>
<div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;        swingStartPos_W,              </div>
<div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;        posHip_W,                     </div>
<div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;        posST_W,                      </div>
<div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;        hip_r_pos_W,                  </div>
<div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;        hip_l_pos_W,                  </div>
<div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;        dq;                           </div>
<div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;    Eigen::VectorXd stanceStartPos_W; </div>
<div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;    Eigen::MatrixXd fe_r_rot_W,       </div>
<div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;        fe_l_rot_W;                   </div>
<div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;    Eigen::MatrixXd dyn_M,            </div>
<div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;        dyn_Non,                      </div>
<div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;        J_l,                          </div>
<div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;        J_r,                          </div>
<div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;        dJ_l,                         </div>
<div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;        dJ_r;                         </div>
<div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;    <span class="keywordtype">double</span> theta0;                    </div>
<div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;    <span class="keywordtype">int</span> model_nv;                     </div>
<div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;};</div>
<div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160; </div>
<div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160; </div>
</div><!-- fragment --></div><!-- contents -->
<div class="ttc" id="aclassGaitScheduler_html_af10903d6376bdc95bfe49c41ff951017"><div class="ttname"><a href="classGaitScheduler.html#af10903d6376bdc95bfe49c41ff951017">GaitScheduler::legState</a></div><div class="ttdeci">DataBus::LegState legState</div><div class="ttdoc">leg state (swing or stance)</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00028">gait_scheduler.h:28</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_a614ce3fed948dcc2746f21202437df62"><div class="ttname"><a href="classGaitScheduler.html#a614ce3fed948dcc2746f21202437df62">GaitScheduler::FLest</a></div><div class="ttdeci">Eigen::VectorXd FLest</div><div class="ttdoc">estimate of left foot ground reaction force</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00049">gait_scheduler.h:49</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_a3473035ec27c4cdee3d04b1bd156ac0e"><div class="ttname"><a href="classGaitScheduler.html#a3473035ec27c4cdee3d04b1bd156ac0e">GaitScheduler::isIni</a></div><div class="ttdeci">bool isIni</div><div class="ttdoc">initialization indicator</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00021">gait_scheduler.h:21</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_a1f1a221fb6fa50a17a941990cb8a428f"><div class="ttname"><a href="classGaitScheduler.html#a1f1a221fb6fa50a17a941990cb8a428f">GaitScheduler::phi</a></div><div class="ttdeci">double phi</div><div class="ttdoc">phase varialbe for gait and swing trajectory generation, must between 0 and 1</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00022">gait_scheduler.h:22</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_aa23623812735c74e6e97554489599790"><div class="ttname"><a href="classGaitScheduler.html#aa23623812735c74e6e97554489599790">GaitScheduler::step</a></div><div class="ttdeci">void step()</div><div class="ttdoc">update the gait</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8cpp_source.html#l00067">gait_scheduler.cpp:67</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_a23b4fe5d102f13755d2b14950bc13e3b"><div class="ttname"><a href="classGaitScheduler.html#a23b4fe5d102f13755d2b14950bc13e3b">GaitScheduler::Fz_L_m</a></div><div class="ttdeci">double Fz_L_m</div><div class="ttdoc">measured ground reaction force along z-axis of left foot</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00026">gait_scheduler.h:26</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_a81aa9cc7644286cb50f5f7d2c066d5b3"><div class="ttname"><a href="classGaitScheduler.html#a81aa9cc7644286cb50f5f7d2c066d5b3">GaitScheduler::stop</a></div><div class="ttdeci">void stop()</div><div class="ttdoc">stop the gait</div></div>
<div class="ttc" id="aclassGaitScheduler_html_ab43cec46486e1e5f3cd6e59a6c162ac7"><div class="ttname"><a href="classGaitScheduler.html#ab43cec46486e1e5f3cd6e59a6c162ac7">GaitScheduler::Fz_R_m</a></div><div class="ttdeci">double Fz_R_m</div><div class="ttdoc">measured ground reaction force along z-axis of right foot</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00027">gait_scheduler.h:27</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_a91185099fb4d6ad3154c01426cd841dc"><div class="ttname"><a href="classGaitScheduler.html#a91185099fb4d6ad3154c01426cd841dc">GaitScheduler::FzThrehold</a></div><div class="ttdeci">double FzThrehold</div><div class="ttdoc">the ground reaction force threhold along z-axis used to indicate the foot has fallen to the ground co...</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00025">gait_scheduler.h:25</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_a7593fedd09b2c3897b9a81db2bb0560a"><div class="ttname"><a href="classGaitScheduler.html#a7593fedd09b2c3897b9a81db2bb0560a">GaitScheduler::legStateNext</a></div><div class="ttdeci">DataBus::LegState legStateNext</div><div class="ttdoc">next leg state</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00029">gait_scheduler.h:29</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_ae7e444a24e0ed19c0a52b8832220538f"><div class="ttname"><a href="classGaitScheduler.html#ae7e444a24e0ed19c0a52b8832220538f">GaitScheduler::tSwing</a></div><div class="ttdeci">double tSwing</div><div class="ttdoc">swing period</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00023">gait_scheduler.h:23</a></div></div>
<div class="ttc" id="astructDataBus_html_a0dcb8a4552bb6b37259a61d601cd7d7e"><div class="ttname"><a href="structDataBus.html#a0dcb8a4552bb6b37259a61d601cd7d7e">DataBus::LegState</a></div><div class="ttdeci">LegState</div><div class="ttdoc">enum current leg stance states</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00164">data_bus.h:164</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_abbd1d35129e19e01297f883b69f6cbd3"><div class="ttname"><a href="classGaitScheduler.html#abbd1d35129e19e01297f883b69f6cbd3">GaitScheduler::FRest</a></div><div class="ttdeci">Eigen::VectorXd FRest</div><div class="ttdoc">estimate of right foot ground reaction force</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00050">gait_scheduler.h:50</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_afe24a16f3a4b9e4f5db04056b3cdc349"><div class="ttname"><a href="classGaitScheduler.html#afe24a16f3a4b9e4f5db04056b3cdc349">GaitScheduler::dt</a></div><div class="ttdeci">double dt</div><div class="ttdoc">contorl frequency</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00024">gait_scheduler.h:24</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html"><div class="ttname"><a href="classGaitScheduler.html">GaitScheduler</a></div><div class="ttdoc">This class to shedule gait, including leg swing and stance-swing transition.</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00019">gait_scheduler.h:19</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_ae80429157cc4a030426a9776a3c087a5"><div class="ttname"><a href="classGaitScheduler.html#ae80429157cc4a030426a9776a3c087a5">GaitScheduler::GaitScheduler</a></div><div class="ttdeci">GaitScheduler(double tSwingIn, double dtIn)</div><div class="ttdoc">Construct a new Gait Scheduler object.</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8cpp_source.html#l00014">gait_scheduler.cpp:14</a></div></div>
<div class="ttc" id="aclassGaitScheduler_html_a5c644ded833c2b83911d70265a51a12c"><div class="ttname"><a href="classGaitScheduler.html#a5c644ded833c2b83911d70265a51a12c">GaitScheduler::torJoint</a></div><div class="ttdeci">Eigen::VectorXd torJoint</div><div class="ttdoc">last control torque on joint, used to estimate GRF</div><div class="ttdef"><b>Definition:</b> <a href="gait__scheduler_8h_source.html#l00051">gait_scheduler.h:51</a></div></div>
<div class="ttc" id="astructDataBus_html"><div class="ttname"><a href="structDataBus.html">DataBus</a></div><div class="ttdoc">The software model for data transporting.</div><div class="ttdef"><b>Definition:</b> <a href="data__bus_8h_source.html#l00022">data_bus.h:22</a></div></div>
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.17
</small></address>
</body>
</html>
